#!/usr/bin/env python
#coding: utf-8

import rospy

import actionlib
from actionlib_msgs.msg import *
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
from nav_msgs.msg import Path
from geometry_msgs.msg import PoseWithCovarianceStamped
from tf_conversions import transformations
from math import pi
from std_msgs.msg import String

from ar_track_alvar_msgs.msg import AlvarMarkers
from ar_track_alvar_msgs.msg import AlvarMarker

from geometry_msgs.msg  import Point
import sys
reload(sys)
sys.setdefaultencoding('utf-8')
import os

id = 255
move_flog = 0

class navigation_demo:
	def __init__(self):
		self.set_pose_pub = rospy.Publisher('/initialpose', PoseWithCovarianceStamped, queue_size=5)
		self.arrive_pub = rospy.Publisher('/voiceWords',String,queue_size=10)
		self.ar_sub = rospy.Subscriber('/object_position', Point, self.ar_cb);
		self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)
		self.move_base.wait_for_server(rospy.Duration(60))
    
	# shi bie
	def ar_cb(self, data):
		global id
		global move_flog
		
		if id != 255 or move_flog == 2:
			return
		
		point_msg = data
        #rospy.loginfo('z = %d', point_msg.z)
		
		imgList = [
			{
				"id": 1,
				"min": 10,
				"max": 19
			},
			{
				"id": 2,
				"min": 20,
				"max": 29
			},
			{
				"id": 3,
				"min": 30,
				"max": 39
			},
			{
				"id": 4,
				"min": 40,
				"max": 49
			},
			{
				"id": 5,
				"min": 50,
				"max": 59
			},
			{
				"id": 6,
				"min": 60,
				"max": 69
			},
			{
				"id": 7,
				"min": 70,
				"max": 79
			},
			{
				"id": 8,
				"min": 80,
				"max": 89
			},
		]
		if point_msg.z != 255 and move_flog == 1: 
			for img in imgList:
				if img['min'] <= point_msg.z <= img['max']:
					id = img['id']
					print("sao miao success -- id = " + str(id))



	def set_pose(self, p):
		if self.move_base is None:
			return False

		x, y, th = p

		pose = PoseWithCovarianceStamped()
		pose.header.stamp = rospy.Time.now()
		pose.header.frame_id = 'map'
		pose.pose.pose.position.x = x
		pose.pose.pose.position.y = y
		q = transformations.quaternion_from_euler(0.0, 0.0, th/180.0*pi)
		pose.pose.pose.orientation.x = q[0]
		pose.pose.pose.orientation.y = q[1]
		pose.pose.pose.orientation.z = q[2]
		pose.pose.pose.orientation.w = q[3]

		self.set_pose_pub.publish(pose)
		return True

	def _done_cb(self, status, result):
		rospy.loginfo("navigation done! status:%d result:%s"%(status, result))
		arrive_str = "arrived to traget point"
		self.arrive_pub.publish(arrive_str)

	def _active_cb(self):
		rospy.loginfo("[Navi] navigation has be actived")

	def _feedback_cb(self, feedback):
		msg = feedback
		#rospy.loginfo("[Navi] navigation feedback\r\n%s"%feedback)

	# action
	def goto(self, p):
		rospy.loginfo("[Navi] goto %s"%p)
		#arrive_str = "going to next point"
		#self.arrive_pub.publish(arrive_str)
		goal = MoveBaseGoal()

		goal.target_pose.header.frame_id = 'map'
		goal.target_pose.header.stamp = rospy.Time.now()
		goal.target_pose.pose.position.x = p[0]
		goal.target_pose.pose.position.y = p[1]
		q = transformations.quaternion_from_euler(0.0, 0.0, p[2]/180.0*pi)
		goal.target_pose.pose.orientation.x = q[0]
		goal.target_pose.pose.orientation.y = q[1]
		goal.target_pose.pose.orientation.z = q[2]
		goal.target_pose.pose.orientation.w = q[3]

		self.move_base.send_goal(goal, self._done_cb, self._active_cb, self._feedback_cb)
		result = self.move_base.wait_for_result(rospy.Duration(60))
		if not result:
			self.move_base.cancel_goal()
			rospy.loginfo("Timed out achieving goal")
		else:
			state = self.move_base.get_state()
			if state == GoalStatus.SUCCEEDED:
				rospy.loginfo("reach goal %s succeeded!"%p)
		return True

	def cancel(self):
		self.move_base.cancel_all_goals()
		return True

if __name__ == "__main__":
	rospy.init_node('navigation_demo',anonymous=True)
	navi = navigation_demo()
	
	print ('Please 1 to continue: ')
	input = raw_input()
	
	if input == '2':
		comm = ''
		while comm != 'exit':
			comm = raw_input()
			if comm in str(range(0, 8)):
				navi.goto(goals[int(comm)])
			else:
				arr = comm.split(' ')
				print(list(arr))
				if len(arr) == 3:
					navi.goto([float(arr[0]), float(arr[1]), float(arr[2])])
	
	if input == '1':
		print('begin')
	
	goals = [
		{ # 1st gird
			"scan": [0.2,-0.55,180], # scan point,
			"d1": [0.37,-1.06,0], # 1
			"d2": [1.05,-0.26,0] # 2
		},
		{ # 2ed gird
			"scan": [2.85,-0.07,0], # scan point,
			"d3": [2.17,-0.5,0], # 1
			"d4": [2.70,-1.13,0] # 2
		},
		{ # 3rd gird
			"scan": [2.80,-3.2,0], # scan point,
			"d5": [2.89,-2.10,0], # 1
			"d6": [2.1,-2.98,0] # 2
		},
		{ # 4th gird
			"scan": [0.15,-2.87,180], # scan point,
			"d7": [1.0,-2.85,180], # 1
			"d8": [0.31,-2.14,180] # 2
		},
	]
	for goal in goals:
		# go scan point
		navi.goto(goal['scan'])
		rospy.sleep(2)

		# shi bie
		move_flog = 1
		while id==255:
			print("wait...")
			rospy.sleep(1)
		move_flog = 2
		print("id = " + str(id))
		
		music_path = "~/mp3/"+"a"+str(id)+".mp3"
		os.system('mplayer %s' % music_path)
		rospy.sleep(2)

		# go work point
		#print('confirm: ' + str(goal['goal']))
			
		print("goal = " + str(goal.get("d"+str(id))))
		navi.goto(goal.get("d"+str(id)) )
		rospy.sleep(2)
		music_path = "~/mp3/"+"b"+str(id)+".mp3"
		os.system('mplayer %s' % music_path)
		rospy.sleep(1)
		id = 255

	print("finish, go end point")
	navi.goto([0.40,-3.05,0])

	music_path = "~/mp3/"+"15"+".mp3"
	os.system('mplayer %s' % music_path)
	rospy.sleep(2)

	print('over!')

	r = rospy.Rate(1)
	while not rospy.is_shutdown():
		  r.sleep()
